/**
  ******************************************************************************
  * @file    queue.c 
  * @author  MLC
  * @version V2.0.2  增加一个返回最早数据的函数queue_get_first_address
  * @date    08-Mar-2020
  * @brief   队列
  */  
    
#include "wingbot_queue.h"
#include <string.h>

/**
  * @brief  初始化队列
  * @param  p_queue:    队列控制块 
            p_arr:      队列数据起始地址 
            size:       队列数据大小
  * @retval None
  */
void queue_init(Queue_T* p_queue, unsigned char* p_arr, unsigned int size)
{
    p_queue->head = 0;
    p_queue->tail = 0;
    p_queue->parr = p_arr;
    p_queue->size = size;
    p_queue->isFull = 0;
    p_queue->length = 0;
}

/**
  * @brief  队列数据清零
  * @retval None
  */
void queue_clear(Queue_T* p_queue)
{
    unsigned int i;
    p_queue->head = 0;
    p_queue->tail = 0;
    p_queue->isFull = 0;
    p_queue->length = 0;
    for(i=0;i<p_queue->size;i++)
        p_queue->parr[i] = 0;
}

/**
  * @brief  返回最早的数据的地址
  * @retval 最早数据的指针
  */
unsigned char* queue_get_first_address(Queue_T* p_queue)
{
    unsigned char* ret = p_queue->parr + p_queue->head;
    return ret;
}

/**
  * @brief  往指定队列中写入一个字节数据
  * @param  data:写入的数据
  * @retval None
  */
void queue_push(Queue_T* p_queue, unsigned char data)
{
    p_queue->parr[p_queue->tail] = data;
    //更新tail
    p_queue->tail++;
    if(p_queue->tail == p_queue->size)
        p_queue->tail = 0;
    
    //更新head
    if(p_queue->tail == p_queue->head)
    {
        p_queue->length = p_queue->size - 1;  //tail指向的是待写入的位置，循环到与head重合则已经满了，tail指向最早的数据，可提取的数据为size-1
        p_queue->head++;
        if(p_queue->head == p_queue->size)
            p_queue->head=0;
    }
    else
        p_queue->length = p_queue->tail >= p_queue->head? p_queue->tail - p_queue->head : \
                                            p_queue->tail + p_queue->size - p_queue->head;
    
    //判断是否存满，当head跑到tail后面去了说明满了
    if(p_queue->tail + 1 == p_queue->head || (p_queue->tail == p_queue->size - 1 && p_queue->head == 0))
        p_queue->isFull = 1;
}

/**
  * @brief  从指定队列中读数据
  * @param  p_queue:        队列控制块
            dest:           存入的地址
            length:         读取的长度
  * @retval 0-成功，非0-失败
  */
int queue_read(Queue_T* p_queue, unsigned char* dest, unsigned int length)
{
    int ret = 0;
    if(length >= p_queue->size)
    {
        //最多读size-1
        ret = -1;
        goto exit;
    }
    else
    {
        p_queue->isFull = 0;  //读了数据之后就不满了
        if(p_queue->head + length > p_queue->size)
        {
            unsigned int len1, len2; //分成两段
            len1 = p_queue->size - p_queue->head;
            len2 = length - len1;
            memcpy(dest, p_queue->parr + p_queue->head, len1);
            memcpy(dest + len1, p_queue->parr, len2);
            p_queue->head = len2;
        }
        else
        {
            memcpy(dest, p_queue->parr + p_queue->head, length);
            p_queue->head += length;
            if(p_queue->head == p_queue->size)
                p_queue->head = 0;
        }
        p_queue->length = p_queue->tail >= p_queue->head? p_queue->tail - p_queue->head : \
                                            p_queue->tail + p_queue->size - p_queue->head;        //当head == tail时数据全部被读取
    }
    exit:
        return ret;
}
    
/*****END OF FILE****/
